#pragma once
#include <assert.h>
#include <math.h>
#define _USE_MATH_DEFINES
#define EPSILON ( 10e-10 )

// Path types
#define LSL ( 0 )
#define LSR ( 1 )
#define RSL ( 2 )
#define RSR ( 3 )
#define RLR ( 4 )
#define LRL ( 5 )
#define L_SEG ( 0 )
#define S_SEG ( 1 )
#define R_SEG ( 2 )

// Error codes
#define EDUBOK ( 0 )         // No error
#define EDUBCOCONFIGS ( 1 )  // Colocated configurations
#define EDUBPARAM ( 2 )      // Path parameterisitation error
#define EDUBBADRHO ( 3 )     // the rho value is invalid
#define EDUBNOPATH ( 4 )     // no connection between configurations with this word

#define UNPACK_INPUTS( alpha, beta ) \
    double sa   = sin( alpha );      \
    double sb   = sin( beta );       \
    double ca   = cos( alpha );      \
    double cb   = cos( beta );       \
    double c_ab = cos( alpha - beta );

#define PACK_OUTPUTS( outputs ) \
    outputs[ 0 ] = t;           \
    outputs[ 1 ] = p;           \
    outputs[ 2 ] = q;

const int DIRDATA[][ 3 ] = { { L_SEG, S_SEG, L_SEG }, { L_SEG, S_SEG, R_SEG }, { R_SEG, S_SEG, L_SEG },
                             { R_SEG, S_SEG, R_SEG }, { R_SEG, L_SEG, R_SEG }, { L_SEG, R_SEG, L_SEG } };

static double fmodr( double x, double y ) {
    return x - y * floor( x / y );
}

static double mod2pi( double theta ) {
    return fmodr( theta, 2 * M_PI );
}

// Only exposed for testing purposes
static int dubins_LSL( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double tmp0      = d + sa - sb;
    double p_squared = 2 + ( d * d ) - ( 2 * c_ab ) + ( 2 * d * ( sa - sb ) );
    if ( p_squared < 0 ) {
        return EDUBNOPATH;
    }
    double tmp1 = atan2( ( cb - ca ), tmp0 );
    double t    = mod2pi( -alpha + tmp1 );
    double p    = sqrt( p_squared );
    double q    = mod2pi( beta - tmp1 );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

static int dubins_RSR( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double tmp0      = d - sa + sb;
    double p_squared = 2 + ( d * d ) - ( 2 * c_ab ) + ( 2 * d * ( sb - sa ) );
    if ( p_squared < 0 ) {
        return EDUBNOPATH;
    }
    double tmp1 = atan2( ( ca - cb ), tmp0 );
    double t    = mod2pi( alpha - tmp1 );
    double p    = sqrt( p_squared );
    double q    = mod2pi( -beta + tmp1 );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

static int dubins_LSR( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double p_squared = -2 + ( d * d ) + ( 2 * c_ab ) + ( 2 * d * ( sa + sb ) );
    if ( p_squared < 0 ) {
        return EDUBNOPATH;
    }
    double p    = sqrt( p_squared );
    double tmp2 = atan2( ( -ca - cb ), ( d + sa + sb ) ) - atan2( -2.0, p );
    double t    = mod2pi( -alpha + tmp2 );
    double q    = mod2pi( -mod2pi( beta ) + tmp2 );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

static int dubins_RSL( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double p_squared = ( d * d ) - 2 + ( 2 * c_ab ) - ( 2 * d * ( sa + sb ) );
    if ( p_squared < 0 ) {
        return EDUBNOPATH;
    }
    double p    = sqrt( p_squared );
    double tmp2 = atan2( ( ca + cb ), ( d - sa - sb ) ) - atan2( 2.0, p );
    double t    = mod2pi( alpha - tmp2 );
    double q    = mod2pi( beta - tmp2 );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

static int dubins_RLR( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double tmp_rlr = ( 6. - d * d + 2 * c_ab + 2 * d * ( sa - sb ) ) / 8.;
    if ( fabs( tmp_rlr ) > 1 ) {
        return EDUBNOPATH;
    }
    double p = mod2pi( 2 * M_PI - acos( tmp_rlr ) );
    double t = mod2pi( alpha - atan2( ca - cb, d - sa + sb ) + mod2pi( p / 2. ) );
    double q = mod2pi( alpha - beta - t + mod2pi( p ) );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

static int dubins_LRL( double alpha, double beta, double d, double *outputs ) {
    UNPACK_INPUTS( alpha, beta );
    double tmp_lrl = ( 6. - d * d + 2 * c_ab + 2 * d * ( -sa + sb ) ) / 8.;
    if ( fabs( tmp_lrl ) > 1 ) {
        return EDUBNOPATH;
    }
    double p = mod2pi( 2 * M_PI - acos( tmp_lrl ) );
    double t = mod2pi( -alpha - atan2( ca - cb, d + sa - sb ) + p / 2. );
    double q = mod2pi( mod2pi( beta ) - alpha - t + mod2pi( p ) );
    PACK_OUTPUTS( outputs );
    return EDUBOK;
}

// The various types of solvers for each of the path types
typedef int ( *DubinsWord )( double, double, double, double * );
// A complete list of the possible solvers that could give optimal paths
static DubinsWord dubins_words[] = {
    dubins_LSL, dubins_LSR, dubins_RSL, dubins_RSR, dubins_RLR, dubins_LRL,
};

typedef struct {
    double qi[ 3 ];     // the initial configuration
    double param[ 3 ];  // the lengths of the three segments
    double rho;         // model forward velocity / model angular velocity
    int    type;        // path type. one of LSL, LSR, ...
} DubinsPath;

typedef int ( *DubinsPathSamplingCallback )( double q[ 3 ], double t, void *user_data );

static int dubins_init_normalised( double alpha, double beta, double d, DubinsPath *path ) {
    double best_cost = INFINITY;
    int    best_word;
    int    i;

    best_word = -1;
    for ( i = 0; i < 6; i++ ) {
        double params[ 3 ];
        //分别调用不同的Dubins函数曲线计算得到t,p, q,存放于params
        int err = dubins_words[ i ]( alpha, beta, d, params );
        if ( err == EDUBOK ) {
            double cost = params[ 0 ] + params[ 1 ] + params[ 2 ];  //三段线段的长度作为代价
            if ( cost < best_cost ) {                               //将最好的结果保存到path->param看
                best_word        = i;
                best_cost        = cost;
                path->param[ 0 ] = params[ 0 ];
                path->param[ 1 ] = params[ 1 ];
                path->param[ 2 ] = params[ 2 ];
                path->type       = i;
            }
        }
    }

    if ( best_word == -1 ) {
        return EDUBNOPATH;
    }
    path->type = best_word;
    return EDUBOK;
}

static int dubins_init( double q0[ 3 ], double q1[ 3 ], double rho, DubinsPath *path ) {
    int    i;
    double dx = q1[ 0 ] - q0[ 0 ];
    double dy = q1[ 1 ] - q0[ 1 ];
    double D  = sqrt( dx * dx + dy * dy );
    double d  = D / rho;  //将两点距离除以最小转弯半径得到归一化距离d
    if ( rho <= 0. ) {
        return EDUBBADRHO;
    }
    double theta = mod2pi( atan2( dy, dx ) );
    double alpha = mod2pi( q0[ 2 ] - theta );
    double beta  = mod2pi( q1[ 2 ] - theta );  //计算坐标原点移到第一个点后，始点和终点的朝向
    for ( i = 0; i < 3; i++ ) {                //将起点放入path->qi变量
        path->qi[ i ] = q0[ i ];
    }
    path->rho = rho;  //最小转弯半径

    return dubins_init_normalised( alpha, beta, d, path );
}

static double dubins_path_length( DubinsPath *path ) {
    double length = 0.;
    length += path->param[ 0 ];
    length += path->param[ 1 ];
    length += path->param[ 2 ];
    length = length * path->rho;
    return length;
}

static int dubins_path_type( DubinsPath *path ) {
    return path->type;
}

static void dubins_segment( double t, double qi[ 3 ], double qt[ 3 ], int type ) {
    assert( type == L_SEG || type == S_SEG || type == R_SEG );
    // Shkel A M, Lumelsky V. Classification of the Dubins set[J]. Robotics and Autonomous Systems, 2001, 34(4):
    // 179-202.
    if ( type == L_SEG ) {  //公式 (1)的第一个式子，此处经过归一化后v=1
        qt[ 0 ] = qi[ 0 ] + sin( qi[ 2 ] + t ) - sin( qi[ 2 ] );
        qt[ 1 ] = qi[ 1 ] - cos( qi[ 2 ] + t ) + cos( qi[ 2 ] );
        qt[ 2 ] = qi[ 2 ] + t;
    } else if ( type == R_SEG ) {  //公式(1)的第二个式子，此处经过归一化后v=1
        qt[ 0 ] = qi[ 0 ] - sin( qi[ 2 ] - t ) + sin( qi[ 2 ] );
        qt[ 1 ] = qi[ 1 ] + cos( qi[ 2 ] - t ) - cos( qi[ 2 ] );
        qt[ 2 ] = qi[ 2 ] - t;
    } else if ( type == S_SEG ) {  //公式(1)的第三个式子，此处经过归一化后v=1
        qt[ 0 ] = qi[ 0 ] + cos( qi[ 2 ] ) * t;
        qt[ 1 ] = qi[ 1 ] + sin( qi[ 2 ] ) * t;
        qt[ 2 ] = qi[ 2 ];
    }
}

static int dubins_path_sample( DubinsPath *path, double t, double q[ 3 ] ) {
    if ( t < 0 || t >= dubins_path_length( path ) ) {
        // error, parameter out of bounds
        return EDUBPARAM;
    }

    // tprime is the normalised variant of the parameter t
    double tprime = t / path->rho;

    // In order to take rho != 1 into account this function needs to be more complex
    // than it would be otherwise. The transformation is done in five stages.
    //
    // 1. translate the components of the initial configuration to the origin
    // 2. generate the target configuration
    // 3. transform the target configuration
    //      scale the target configuration
    //      translate the target configration back to the original starting point
    //      normalise the target configurations angular component

    // The translated initial configuration
    // 将路径放在原点(0, 0)，此时，只需要将角度保留即可
    double qi[ 3 ] = { 0, 0, path->qi[ 2 ] };

    // Generate the target configuration
    // 生成中间点的位置
    const int *types = DIRDATA[ path->type ];
    double     p1    = path->param[ 0 ];  //路径的第一个角度
    double     p2    = path->param[ 1 ];  //路径的第二个角度
    double     q1[ 3 ];                   // end-of segment 1
    double     q2[ 3 ];                   // end-of segment 2
    //从第qi点为起点，根据类型types[0]，生成后一个点q1的configuration（即计算下一个节点的位置）
    dubins_segment( p1, qi, q1, types[ 0 ] );
    //从第q1点为起点，根据类型types[1]，生成后一个点q2的configuration（即计算下一个节点的位置）
    dubins_segment( p2, q1, q2, types[ 1 ] );
    //生成q点的configuration
    if ( tprime < p1 ) {
        dubins_segment( tprime, qi, q, types[ 0 ] );
    } else if ( tprime < ( p1 + p2 ) ) {
        dubins_segment( tprime - p1, q1, q, types[ 1 ] );
    } else {
        dubins_segment( tprime - p1 - p2, q2, q, types[ 2 ] );
    }

    // scale the target configuration, translate back to the original starting point
    q[ 0 ] = q[ 0 ] * path->rho + path->qi[ 0 ];
    q[ 1 ] = q[ 1 ] * path->rho + path->qi[ 1 ];
    q[ 2 ] = mod2pi( q[ 2 ] );

    return 0;
}

static int dubins_path_sample_many( DubinsPath *path, DubinsPathSamplingCallback cb, double stepSize,
                                    void *user_data ) {
    double x      = 0.0;
    double length = dubins_path_length( path );
    while ( x < length ) {
        double q[ 3 ];
        dubins_path_sample( path, x, q );
        int retcode = cb( q, x, user_data );
        if ( retcode != 0 ) {
            return retcode;
        }
        x += stepSize;
    }
    return 0;
}

static int dubins_path_endpoint( DubinsPath *path, double q[ 3 ] ) {
    return dubins_path_sample( path, dubins_path_length( path ) - EPSILON, q );
}

static int dubins_extract_subpath( DubinsPath *path, double t, DubinsPath *newpath ) {
    // calculate the true parameter
    double tprime = t / path->rho;

    // copy most of the data
    newpath->qi[ 0 ] = path->qi[ 0 ];
    newpath->qi[ 1 ] = path->qi[ 1 ];
    newpath->qi[ 2 ] = path->qi[ 2 ];
    newpath->rho     = path->rho;
    newpath->type    = path->type;

    // fix the parameters
    newpath->param[ 0 ] = fmin( path->param[ 0 ], tprime );
    newpath->param[ 1 ] = fmin( path->param[ 1 ], tprime - newpath->param[ 0 ] );
    newpath->param[ 2 ] = fmin( path->param[ 2 ], tprime - newpath->param[ 0 ] - newpath->param[ 1 ] );
    return 0;
}
